
#ifndef SERIAL_PORT_RECEIVER_H_
#define SERIAL_PORT_RECEIVER_H_

#include "queue.h"
#include "string.h"
#include "timeout.h"
#include "usart.h"
#include "crc.h"

#define STOP_CHAR 0x7E
#define ESCAPE_CHAR 0x7D
#define EOT_CHAR 0x04
#define XOR_CHAR 0x20

#define MAXIMUM_SERIAL_PORT_BUFFER_SIZE 200

struct SerialPortPackage
{
  int id;
  String<MAXIMUM_SERIAL_PORT_BUFFER_SIZE> message;
};

template <USART_TEMPLATE>
class SerialPortReceiver
{
  public:
    SerialPortReceiver()
    {
      timeout.restart(150);
      escaping = false;
      buffer.clear();
      this->usart = 0;
    }

    ~SerialPortReceiver(void)
    {
    }

    void setUsart(Usart<USART_TEMPLATE_PARAMS> *usart)
    {
      this->usart = usart;
    }

    void appendChar(unsigned char data)
    {
      // Clear buffer if timed out
      if (timeout.timedOut()) {
        buffer.clear();
      }
      timeout.restart();

      // Handle incoming data
      if (data == STOP_CHAR) {
        escaping = false;
        if (buffer.size() > 1) {
          // Receive message
          unsigned char crc;
          crc = buffer[buffer.size() - 1];
          buffer.substr(&payload, 0, buffer.size() - 1);
          if (crc == crc8(payload)) {
            serial_port_package.id = payload[0];
            if (payload.size() > 1) {
              payload.substr(&(serial_port_package.message), 1);
            }
            serial_port_packages.push(serial_port_package);
          }
        }
        buffer.clear();
      } else if (data == ESCAPE_CHAR) {
        escaping = true;
      } else {
        if (escaping) {
          data = data ^ XOR_CHAR;
          escaping = false;
        }
        if (buffer.size() < buffer.capacity()) {
          buffer += data;
        } else {
          buffer.clear();
        }
      }
    }

    bool getPackage(SerialPortPackage *serial_port_package)
    {
      this->usart->disableRxInterrupt();
      if (serial_port_packages.empty()) {
        this->usart->enableRxInterrupt();
        return false;
      } else {
        *serial_port_package = serial_port_packages.front();
        serial_port_packages.pop();
        this->usart->enableRxInterrupt();
        return true;
      }
    }
  
  private:
    Usart<USART_TEMPLATE_PARAMS> *usart;
    bool escaping;
    int character_count;
    String<MAXIMUM_SERIAL_PORT_BUFFER_SIZE> buffer;
    String<MAXIMUM_SERIAL_PORT_BUFFER_SIZE> payload;
    static const int packages_max_count = 20;
    SerialPortPackage serial_port_package;
    Queue<SerialPortPackage, packages_max_count> serial_port_packages;
    Timeout timeout;
};

template <USART_TEMPLATE>
class SerialPortTransmitter
{
  public:
    SerialPortTransmitter()
    {
      this->usart = 0;
      this->useCrc = false;
    }

    void setUsart(Usart<USART_TEMPLATE_PARAMS> *usart, bool useCrc)
    {
      this->usart = usart;
      this->useCrc = useCrc;
    }

    void send(const SerialPortPackage &serial_port_package)
    {
      unsigned char crc;
      sendChar(STOP_CHAR, false);
      sendChar(serial_port_package.id, true);
      crc = crc8(0, serial_port_package.id);
      for (int idx = 0; idx < serial_port_package.message.size(); idx++) {
        unsigned char data(serial_port_package.message[idx]);
        sendChar(data, true);
        crc = crc8(crc, data);
      }
      sendChar(crc, true);
      sendChar(STOP_CHAR, false);
    }

    void sendChar(unsigned char data, bool escape)
    {
      if (escape && (data == 0x00 || data == 17 || data == 255 || data == EOT_CHAR || data == ESCAPE_CHAR || data == STOP_CHAR)) {
        usart->sendChar(ESCAPE_CHAR);
        usart->sendChar(data ^ XOR_CHAR);
      } else {
        usart->sendChar(data);
      }
    }

  private:
    bool useCrc;
    Usart<USART_TEMPLATE_PARAMS> *usart;
};

#endif
